#ifndef FHE_MATH_MAT_H
#define FHE_MATH_MAT_H

#include <fhe/math/Math.h>
#include <fhe/core/FileSystem.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <typeinfo>

namespace fhe
{
  namespace core
  {
    template <typename T>
    class Mat<2,T>
    {
    public:
      typedef Vec<2,T> V;
      typedef typename V::R R;
      
      Mat()
      {
        operator=( IDENTITY );
      }
      
      Mat( T d0, T d1, T d2,
           T d3, T d4, T d5,
           T d6, T d7, T d8 )
      {
        d_[0] = d0;
        d_[1] = d1;
        d_[2] = d2;
        d_[3] = d3;
        d_[4] = d4;
        d_[5] = d5;
        d_[6] = d6;
        d_[7] = d7;
        d_[8] = d8;
      }
      
      Mat( const Mat& m )
      {
        for ( size_t i = 0; i < 9; ++i )
        {
          d_[i] = m.d_[i];
        }
      }
      
      Mat& operator=( const Mat& m )
      {
        for ( size_t i = 0; i < 9; ++i )
        {
          d_[i] = m.d_[i];
        }
        return *this;
      }
      
      T operator()( size_t i, size_t j ) const
      {
        FHE_ASSERT( i < 3 && j < 3 );
        return d_[ i * 3 + j ];
      }
      
      T& operator()( size_t i, size_t j )
      {
        FHE_ASSERT( i < 3 && j < 3 );
        return d_[ i * 3 + j ];
      }
      
      Mat operator*( const Mat& m ) const
      {
        Mat r = ZERO;
        for ( size_t i = 0; i < 3; ++i )
        {
          for ( size_t j = 0; j < 3; ++j )
          {
            for ( size_t k = 0; k < 3; ++k )
            {
              r(i,j) += operator()(i,k) * m(k,j);
            }
          }
        }
        return r;
      }
      
      V operator*( const V& v ) const
      {
        return V( d_[0] * v.x + d_[1] * v.y + d_[2],
                         d_[3] * v.x + d_[4] * v.y + d_[5] );
      }
      
      bool operator==( const Mat& m ) const
      {
        for ( size_t i = 0; i < 9; ++i )
        {
          if ( !Math::equal( d_[i], m.d_[i] ) )
          {
            return false;
          }
        }
        return true;
      }
      
      bool operator!=( const Mat& m ) const
      {
        return !operator==(m);
      }
      
      T det() const
      {
        return d_[0] * (d_[4] * d_[8] - d_[7] * d_[5]) -
               d_[1] * (d_[3] * d_[8] - d_[6] * d_[5]) +
               d_[2] * (d_[3] * d_[7] - d_[6] * d_[4]);
      }

      Mat inverse() const
      {
        T d = det();
        FHE_ASSERT( !Math::equal( d, 0 ) );
        return Mat( (d_[4] * d_[8] - d_[5] * d_[7]) / d,
                    -(d_[1] * d_[8] - d_[7] * d_[2]) / d,
                    (d_[1] * d_[5] - d_[4] * d_[2]) / d,
                    -(d_[3] * d_[8] - d_[5] * d_[6]) / d,
                    (d_[0] * d_[8] - d_[6] * d_[2]) / d,
                    -(d_[0] * d_[5] - d_[3] * d_[2]) / d,
                    (d_[3] * d_[7] - d_[6] * d_[4]) / d,
                    -(d_[0] * d_[7] - d_[6] * d_[1]) / d,
                    (d_[0] * d_[4] - d_[1] * d_[3]) / d);
      }
      
      static Mat translation( const V& v )
      {
        return Mat( 1, 0, v.x,
                    0, 1, v.y,
                    0, 0, 1 );
      }
      
      static Mat rotation( const R& r )
      {
        T sa = Math::sin( -r.radians() );
        T ca = Math::cos( -r.radians() );
        return Mat( ca, sa, 0,
                    -sa, ca, 0,
                    0, 0, 1 );
      }
      
      static Mat scale( const V& v )
      {
        return Mat( v.x, 0, 0,
                    0, v.y, 0,
                    0, 0, 1 );
      }
      
      //these suck pretty bad
      
      V getTranslation() const
      {
        return V( operator()(0,2), operator()(1,2) );
      }
      
      R getRotation() const
      {
        return R::fromRadians( Math::acos( operator()(0,0) ) );
      }
      
      V getScale() const
      {
        return V( operator()(0,0), operator()(1,1) );
      }
      
      static std::string typeName() 
      {
        return std::string( "Mat2" ) + typeid(T).name();
      }
      
      std::string toString() const
      {
        std::ostringstream os;
        os << typeName() << "(";
        for ( size_t i = 0; i < 9; ++i )
        {
          os << d_[i];
          if ( i < 8 )
          {
            os << ", ";
          }
        }
        os << ")";
        return os.str();
      }
      
      static void serialize( const Data::Ptr& data, Mat m )
      {
        const Data::Ptr d = data->addChild( "d" );
        for ( size_t i = 0; i < 9; ++i )
        {
          std::ostringstream os;
          os << "d" << i;
          FileSystem::instance().serialize( d->addChild( os.str() ), m.d_[i] );
        }
      }
      
      static Mat deserialize( const Data::Ptr& data )
      {
        Mat m = IDENTITY;
        if ( data->hasChild( "d" ) )
        {
          const Data::Ptr d = data->getChild( "d" );
          for ( size_t i = 0; i < 9; ++i )
          {
            std::ostringstream os;
            os << "d" << i;
            const std::string key = os.str();
            if ( d->hasChild( key ) )
            {
              m.d_[i] = FileSystem::instance().deserialize<T>( d->getChild( key ) );
            }
          }
        }
        else
        {
          const V t = data->hasChild( "translation" ) ? 
            FileSystem::instance().deserialize<V>( data->getChild( "translation" ) ) :
            V::ZERO;
          const R r = data->hasChild( "rotation" ) ?
            FileSystem::instance().deserialize<R>( data->getChild( "rotation" ) ) :
            R::IDENTITY;
          const V s = data->hasChild( "scale" ) ?
            FileSystem::instance().deserialize<V>( data->getChild( "scale" ) ) :
            V::ONE;
          m = translation( t ) * rotation( r ) * scale( s );
        }
        return m;
      }
      
      static const Mat IDENTITY;
      static const Mat ZERO;
      
    private:
      T d_[9];
    };
    
    template <typename T>
    const Mat<2,T> Mat<2,T>::IDENTITY( 1, 0, 0,
                                       0, 1, 0,
                                       0, 0, 1 );
    
    template <typename T>
    const Mat<2,T> Mat<2,T>::ZERO( 0, 0, 0,
                                   0, 0, 0,
                                   0, 0, 0 );
    
    template <typename T>
    class Mat<3,T>
    {
    public:
      typedef Vec<3,T> V;
      typedef typename V::R R;
      
      Mat()
      {
        operator=( IDENTITY );
      }
      
      Mat( T d0, T d1, T d2, T d3,
           T d4, T d5, T d6, T d7,
           T d8, T d9, T d10, T d11,
           T d12, T d13, T d14, T d15 )
      {
        d_[0] = d0;
        d_[1] = d1;
        d_[2] = d2;
        d_[3] = d3;
        d_[4] = d4;
        d_[5] = d5;
        d_[6] = d6;
        d_[7] = d7;
        d_[8] = d8;
        d_[9] = d9;
        d_[10] = d10;
        d_[11] = d11;
        d_[12] = d12;
        d_[13] = d13;
        d_[14] = d14;
        d_[15] = d15;
      }
      
      Mat( const Mat& m )
      {
        operator=( m );
      }
      
      Mat& operator=( const Mat& m )
      {
        for ( size_t i = 0; i < 16; ++i )
        {
          d_[i] = m.d_[i];
        }
        return *this;
      }
      
      T operator()( size_t i, size_t j ) const
      {
        FHE_ASSERT( i < 4 && j < 4 );
        return d_[ i * 4 + j ];
      }
      
      T& operator()( size_t i, size_t j )
      {
        FHE_ASSERT( i < 4 && j < 4 );
        return d_[ i * 4 + j ];
      }

      Mat operator*( const Mat& m ) const
      {
        Mat r = ZERO;
        for ( size_t i = 0; i < 4; ++i )
        {
          for ( size_t j = 0; j < 4; ++ j )
          {
            for ( size_t k = 0; k < 4; ++k )
            {
              r(i,j) += operator()(i,k) * m(k,j);
            }
          }
        }
        return r;
      }
      
      V operator*( const V& v ) const
      {
        return V( d_[0] * v.x + d_[1] * v.y + d_[2] * v.z + d_[3],
                  d_[4] * v.x + d_[5] * v.y + d_[6] * v.z + d_[7],
                  d_[8] * v.x + d_[9] * v.y + d_[10] * v.z + d_[11] );

      }
      
      bool operator==( const Mat& m ) const
      {
        for ( size_t i = 0; i < 16; ++i )
        {
          if ( !Math::equal( d_[i], m.d_[i] ) )
          {
            return false;
          }
        }
        return true;
      }
      
      bool operator!=( const Mat& m ) const
      {
        return !operator==(m);
      }
      
      static Mat translation( const V& t )
      {
        return Mat( 1, 0, 0, t.x,
                    0, 1, 0, t.y,
                    0, 0, 1, t.z,
                    0, 0, 0, 1 );
      }
      
      static Mat scale( const V& s )
      {
        return Mat( s.x, 0, 0, 0,
                    0, s.y, 0, 0,
                    0, 0, s.z, 0,
                    0, 0, 0, 1 );
      }
      
      static Mat rotation( const R& r )
      {
        T xx = r.x * r.x,
            xy = r.x * r.y,
            xz = r.x * r.z,
            xw = r.x * r.w,
            yy = r.y * r.y,
            yz = r.y * r.z,
            yw = r.y * r.w,
            zz = r.z * r.z,
            zw = r.z * r.w;
        return Mat(1 - 2 * (yy + zz),  //0
                    2 * (xy - zw),      //1
                    2 * (xz + yw),      //2
                    0,                  //3
                    2 * (xy + zw),      //4
                    1 - 2 * (xx + zz),  //5
                    2 * (yz - xw),      //6
                    0,                  //7
                    2 * (xz - yw),      //8
                    2 * (yz + xw),      //9
                    1 - 2 * (xx + yy),  //10
                    0,                  //11
                    0,0,0,1);
      }
      
      V getTranslation() const
      {
        return V( d_[3], d_[7], d_[11] );
      }
      
      V getScale() const
      {
        return V( d_[0], d_[5], d_[10] );
      }
      
      R getRotation() const
      {
        T t = 1 + d_[0] + d_[5] + d_[10];
        if (t > Math::EPS)
        {
            T s = Math::sqrt(t) * 2.0;
            return R(0.25 * s,
                        (d_[9] - d_[6]) / s,
                        (d_[2] - d_[8]) / s,
                        (d_[4] - d_[1]) / s);
        }
        else if (d_[0] > d_[5] && d_[0] > d_[10])
        {
            T s = Math::sqrt(1.0 + d_[0] - d_[5] - d_[10]) * 2.0;
            return R((d_[9] - d_[6]) / s,
                        0.25 * s,
                        (d_[4] + d_[1]) / s,
                        (d_[2] + d_[8]) / s);
        }
        else if (d_[5] > d_[10])
        {
            T s = Math::sqrt(1.0 + d_[5] - d_[0] - d_[10]) * 2.0;
            return R((d_[2] - d_[8]) / s,
                        (d_[4] + d_[1]) / s,
                        0.25 * s,
                        (d_[2] - d_[8]) / s);
        }
        else
        {
            T s = Math::sqrt(1.0 + d_[10] - d_[0] - d_[5]) * 2.0;
            return R((d_[4] - d_[1]) / s,
                        (d_[2] + d_[8]) / s,
                        (d_[9] + d_[6]) / s,
                        0.25 * s);
        }
      }

      Mat<2,T> submat( size_t i, size_t j ) const
      {
          Mat<2,T> m;
          size_t si, sj, di, dj;
          for (di = 0; di < 3; ++di)
          {
              for (dj = 0; dj < 3; ++dj)
              {
                  if (di >= i)
                      si = di + 1;
                  else
                      si = di;
                  if (dj >= j)
                      sj = dj + 1;
                  else
                      sj = dj;
                  m(di,dj) = operator()(si,sj);
              }
          }
          return m;
      }
      
      T det() const
      {
          T result = 0;
          int i = 1;
          for (size_t n = 0; n < 4; ++n)
          {
              result += d_[n*4] * submat(n,0).det() * i;
              i *= -1;
          }
          return result;
      }
      
      Mat inverse() const
      {
          T d = det();
          if (Math::abs(d) < Math::EPS )
              return IDENTITY;
          else
          {
              Mat m;
              size_t i, j;
              int sign;
              for (i = 0; i < 4; ++i)
                  for (j = 0; j < 4; ++j) 
                  {
                      sign = 1 - ((i + j) % 2) * 2;
                      m.d_[i + j*4] = (submat(i,j).det() * sign) / d;
                  }
              return m;
          }
      }    
      
      static std::string typeName()
      {
        return std::string( "Mat3" ) + typeid(T).name();
      }
      
      std::string toString() const
      {
        std::ostringstream os;
        os << typeName() << "(";
        for ( size_t i = 0; i < 16; ++i )
        {
          os << d_[i];
          if ( i < 15 )
          {
            os << ", ";
          }
        }
        os << ")";
        return os.str();
      }
      
      static void serialize( const Data::Ptr& data, Mat m )
      {
        const Data::Ptr d = data->addChild( "d" );
        for ( size_t i = 0; i < 16; ++i )
        {
          std::ostringstream os;
          os << "d" << i;
          FileSystem::instance().serialize( d->addChild( os.str() ), m.d_[i] );
        }
      }
      
      static Mat deserialize( const Data::Ptr& data )
      {
        Mat m = IDENTITY;
        if ( data->hasChild( "d" ) )
        {
          const Data::Ptr d = data->getChild( "d" );
          for ( size_t i = 0; i < 16; ++i )
          {
            std::ostringstream os;
            os << "d" << i;
            const std::string key = os.str();
            if ( d->hasChild( key ) )
            {
              m.d_[i] = FileSystem::instance().deserialize<T>( d->getChild( key ) );
            }
          }
        }
        else
        {
          const V t = data->hasChild( "translation" ) ? 
            FileSystem::instance().deserialize<V>( data->getChild( "translation" ) ) :
            V::ZERO;
          const R r = data->hasChild( "rotation" ) ?
            FileSystem::instance().deserialize<R>( data->getChild( "rotation" ) ) :
            R::IDENTITY;
          const V s = data->hasChild( "scale" ) ?
            FileSystem::instance().deserialize<V>( data->getChild( "scale" ) ) :
            V::ONE;
          m = translation( t ) * rotation( r ) * scale( s );
        }
        return m;
      }
      
      static const Mat IDENTITY;
      static const Mat ZERO;

    private:
      T d_[16];
    };
    
    template <typename T>
    const Mat<3,T> Mat<3,T>::IDENTITY( 1, 0, 0, 0,
                                       0, 1, 0, 0,
                                       0, 0, 1, 0,
                                       0, 0, 0, 1 );
    
    template <typename T>
    const Mat<3,T> Mat<3,T>::ZERO( 0, 0, 0, 0,
                                   0, 0, 0, 0,
                                   0, 0, 0, 0,
                                   0, 0, 0, 0 );
    
    template <size_t D, typename T>
    std::ostream& operator<<( std::ostream& os, const Mat<D,T>& m )
    {
      return os << m.toString();
    }
    
    typedef Mat<2,double> Mat2d;
    typedef Mat<2,int> Mat2i;
    typedef Mat<3,double> Mat3d;
    typedef Mat<3,int> Mat3i;
  }
}

#endif
